www.gusucode.com > VC 3D弹道仿真程序源码文件-源码程序 > VC 3D弹道仿真程序源码文件-源码程序/code/GPSModel.cpp

    //Download by http://www.NewXing.com
/********************************************************************************
****                     名称：GPS仿真模块                                  ***** 
****                     编写：孙瑞胜
****                     日期：2009年11月8日                               *****
*********************************************************************************/
/******************************************************
函数功能描述：GPS单元仿真器
/*******************************************************
注： Fb=(I+Ka)[I+(dCab)']*fb+零偏＋一介马尔可夫过程  + 正态分布白噪声     
     Wb=(I+Kg)[I+(dCgb)']*wb+零漂＋一介马尔可夫过程  +正态分布白噪声  
*******************************************************/
#include "stdafx.h"

#include "simGPS.h"
#include "ReferenceFrame.h"

extern double t_step;                       //时间步长
extern void Rands2( int &IX, double &YEL, double S, double AM, double &V );//正态分布; 

CGPS::CGPS()
{
	x = 0.0 ;    	y = 0.0 ;     	z = 0.0;
	Vx  = 0.0 ;	    Vy  = 0.0 ;	    Vz  = 0.0 ;
	Ax  = 0.0 ;	    Ay  = 0.0 ;	    Az  = 0.0 ;

}

CGPS::~CGPS()
{
}
void CGPS::SetGPS( ) //白噪声
{

}


void CGPS::Get_err_GPS( double dGPS[]  )
{
	unsigned seeds ;
	static long int NumCount = 0 , nNUM = 1 ; 

	if ( !NumCount )
	{
		seeds = (unsigned)time( NULL ) % 100 ; 
	}
	static int IXX[3]  = { seeds, seeds + 25, seeds +100 };
//	static int IXX[3]  = { 175, 150, 190 };

	static double Sigma[3] = { 2.0, 5.0, 2.0 },  //GPS的均方差取10m, 15m
//	static double Sigma[3] = { 0.0, 0.0, 0.0 },  //GPS的均方差取10m, 15m
		             AM[3] = { 0.0, 0.0, 0.0 };   //GPS的均值取0.0m
	static double YELL[3], Value[3], old_Value[3];

	static ofstream fGPSRand( "simGPS.txt" );    //随机数文件
	double  LIM_GPS ;

/**/
 
	fGPSRand << NumCount << "       " << nNUM << "       ";	
	for ( int i = 0; i < 3; ++ i )
	{
		if ( NumCount % 50 == 0 )         //每1s更新一组
		{
			Rands2( IXX[i], YELL[i], Sigma[i], AM[i], Value[i] ) ;  //生成随机干扰
			
			LIM_GPS = fabs(AM[i])+3*Sigma[i] ;  //干扰风的极限值
			if ( fabs(Value[i]) > LIM_GPS )  
			{
				Value[i] = LIM_GPS * fabs(Value[i]) / Value[i] ;
			}
			nNUM = 1 ;
		}
		dGPS[i] = old_Value[i] + ( Value[i] - old_Value[i] ) * nNUM / 50 ;    // GPS误差分解到每一步
		
		fGPSRand << dGPS[i] << "       " ;	
		if ( nNUM  == 50 )
		{	
			old_Value[i] = Value[i] ;	
		}
	}
	NumCount ++ ; 		nNUM ++ ;	
	fGPSRand << endl;	

	return ; 
}
void CGPS::CalGPSModel( double XS[], double alpha/*射击方位角/rad*/ ) 
{
//	double GPS_c[6], double GPS[6] ;

	Pos_l[0] = XS[1] ; 	Pos_l[1] = XS[2] ; 	Pos_l[2] = XS[3] ;  //x,y,z
	Vel_l[0] = XS[4] ; 	Vel_l[1] = XS[5] ; 	Vel_l[2] = XS[6] ;  //Vx,Vy,Vz

	CFrameofRef *pRefFrame = new CFrameofRef;

	pRefFrame->Ground2Nav( alpha, Vel_l, Vel_n ) ; //发射系下的速度转到导航系
	pRefFrame->Ground2ECEF( alpha, Pos_l, Pos_ecef ) ; //发射系下的位置转到ecef系

	Get_err_GPS( dGPS ) ;  //获得GPS误差，包括位置和速度

	for ( int i = 0; i < 3; ++i )
	{
		Pos_ecef[i] = Pos_ecef[i] + dGPS[i] ;
		Vel_n[i] = Vel_n[i] + dGPS[i+3] ;
	}	
	pRefFrame->ECEF_2_WGS84( Pos_ecef, Pos_WGS84 ) ; //ecef系转到WGS84系

	ReadGPSData( GPS_c, GPS ) ;

	UpdateGPSData( GPS ) ;

	return ;
}

void CGPS::ReadGPSData( double GPS_c[6], double GPS[6]  )
{
	double dGPS[6] ;
	Get_err_GPS( dGPS ) ;

	for ( int i = 0; i < 6; ++i )
	{
		GPS[i] = GPS_c[i] + dGPS[i] ;
	}

	return ;
}
void CGPS::	UpdateGPSData( double GPS[6] )
{
	x = GPS[0] ;  y = GPS[1] ;  z = GPS[2] ;

	return ;
}